#include "ComplexModalSolver.h"

#include "Utility/Algebra/Config.h"
#include "Utility/Configuration/SGParameter.h"
#include "Utility/Exception/SGException.h"
#include "Utility/Logging/SGLogger.h"
#include "Utility/Algebra/SparseBlas.h"
#include "Utility/Algebra/sgArpack.h"

#include "DBManager/IService/Input/Dynamic/IEIGCService.h"
#include "DBManager/IService/Input/Parameter/IParameterService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"

#include "SGFem/Task/Kernel/MModel.h"

#include "SGFem/Constraint/RElement/BoundaryCondition.h"

using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::Status;
using SG::DataStructure::FEM::SOLType;
using SG::DataStructure::FEM::Project;
using SG::DataStructure::FEM::Subcase;

using namespace SG::DBManager;
using namespace Kernel;
using namespace SG::Algebra;
using namespace SG::DataStructure::Common;

void ComplexModalSolver::Solve (const DBServiceFactorySP dbServiceFactory, const Project& project, int subcaseId)
{
    auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    Subcase subcase(para);

    // 模态分析参数
    SG::Utility::SGParameter setting;
    setting.AddInt ("SubcaseID", subcaseId);
    setting.AddString ("FrequencySelectionMode", "LM");  //"LM"：低频优先，"SM"：高频优先

    // 获取复模态求解参数
    SG::DataStructure::FEM::EIGCData EIGCData;
    auto                             status = dbServiceFactory->get<IEIGCService> ()->FindBySId (subcase.GetCMethodId (), EIGCData);
    SGInfo << "Searching for EIGC parameters with Method ID: " << subcase.GetCMethodId();
    if (Status::STATUS_Failure == status)
    {
        SGError << "can't find ModesOrder setting";
        return;
    }
    int nMode = EIGCData.m_ND1;

    // 获取复模态求解参数
    Kernel::MModel         model (SG::DataStructure::FEM::SOLType::SOL_SEDCEIG, dbServiceFactory);
    model.Initialize ({subcaseId});
    model.CalcMatrixEquation (subcaseId);

    SpMat_t K_model, M_model, B_model;
    model.ComputeStiffnessMatrix (K_model);  // 计算刚度矩阵
    model.ComputeMassMatrix (M_model);       // 计算质量矩阵
    model.ComputeDampingMatrix (B_model);    // 瑞利阻尼矩阵

    int            nDof = K_model.rows ();
    ComplexSpMat_t K (2 * nDof, 2 * nDof);
    // 组装 K (非对称)
    SparseBlas::setValues (K, 0, 0, B_model);
    SparseBlas::setValues (K, 0, nDof, K_model);
    SparseBlas::setValues (K, nDof, 0, -M_model);
    // assembly K
    ComplexSpMat_t M (2 * nDof, 2 * nDof);
    SparseBlas::setValues (M, 0, 0, M_model);
    SparseBlas::setValues (M, nDof, nDof, M_model);

    ComplexMat_t phi (nDof, nMode);  // 特征向量
    ComplexMat_t vel (nDof, nMode);  // 速度向量
    ComplexVec_t omega2 (nMode);     // 特征值

    TArpack<Complex_t> arpack;
    auto               opt = arpack.getDefaultOption ();
    opt.nev                = nMode;  // Number of eigenvalues of OP to be computed. 0 < NEV < N.
    opt.which              = setting.GetValue ("FrequencySelectionMode").GetString ();
    arpack.setOption (opt);

    std::vector<SG::Algebra::Complex_t>    eigenvalues;
    std::vector<SG::Algebra::ComplexVec_t> eigenvectors;
    arpack.solve (-K, M, eigenvalues, eigenvectors);  // E是2mm（自由度）行，n+1列矩阵，前n列是特征向量，最后一列特征值从上到下排列

    for (int j = 0; j < nMode; ++j)
    {
        const auto& vec       = eigenvectors[j];
        omega2 (j)            = eigenvalues[j];  // 填充特征值
        size_t maxIndex       = 0;
        double maxMagnitudeSq = 0.0;
        for (int i = 0; i < nDof; ++i)
        {
            vel (i, j) = vec (i);                   // 填充速度向量
            phi (i, j) = vec (i + nDof);            // 填充特征向量

            double magSq = std::norm (phi (i, j));  // std::norm 计算幅值的平方
            if (magSq > maxMagnitudeSq)
            {
                maxMagnitudeSq = magSq;
                maxIndex       = i;
            }
        }

        const double maxMagnitude = std::sqrt (maxMagnitudeSq);

        if (maxMagnitude > 1e-12)
        {
            const double theta          = std::arg (phi (maxIndex, j));
            const auto   combinedFactor = std::polar (1.0 / maxMagnitude, -theta);
            for (int row = 0; row < nDof; ++row)
            {
                phi (row, j) *= combinedFactor;
            }
        }
    }

    Vec_t freq (nMode);  
    Vec_t damp (nMode);  

    double epsilon = 1e-6;
    for (int i = 0; i < nMode; ++i)
    {
        const auto& w2 = omega2[i];
        freq[i]        = std::fabs (w2.imag ()) / (2.0 * M_PI);

        if (std::fabs (w2.imag ()) < epsilon)
        {
            damp[i] = 0.0;
        }
        else
        {
            damp[i] = -2.0 * w2.real () / std::fabs (w2.imag ());
        }

        model.SetEigenValue (i + 1, w2, freq[i], damp[i], -1.0);
    }

    int current_subcase = setting.GetValue ("SubcaseID").GetInt ();
    model.SetEigenVector (phi, omega2, current_subcase);
    

}